﻿using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using static MAVLink;

namespace DroneLib.Communication
{
    public abstract class MavStream:Stream
    {
        internal string _Name;
        public string Name => _Name;

        public override bool CanRead => true;

        public override bool CanSeek => false;

        public override bool CanWrite => true;

        public override long Position { get => throw new NotSupportedException(); set => throw new NotSupportedException(); }

        protected bool _Connected = false;
        public virtual bool Connected => _Connected;

        public virtual bool IsOpen { get; protected set; } = false;

        public bool ConnectionFailed { get; protected set; } = false;
        public string Error { get; protected set; } = string.Empty;

        public int MavlinkVersion { get; set; } = 2;

        public int HeartbeatInterval { get; set; } = 1000;

        public byte GcsID { get; set; } = 255;

        public override long Seek(long offset, SeekOrigin origin)
        {
            throw new NotSupportedException();
        }

        public override void SetLength(long value)
        {
            throw new NotSupportedException();
        }

        public abstract void Open();
        public abstract override void Close();

        public abstract void Reconnect();

        public delegate void LinkBrokenEventHandler(MavStream mavStream);
        public event LinkBrokenEventHandler LinkBroken;

        public delegate void LinkConnectedEventHandler(MavStream mavStream, bool connected);
        public event LinkConnectedEventHandler LinkConnected;

        protected DateTime _LastDataTime = DateTime.MinValue;

        public TimeSpan TimeSinceLastData { get => DateTime.Now.Subtract(_LastDataTime); }

        ConcurrentQueue<MAVLink.MAVLinkMessage> _RxMessageQueue = new ConcurrentQueue<MAVLink.MAVLinkMessage>();
        MAVLink.MavlinkParse _Parse = new MAVLink.MavlinkParse();
        protected bool _TerminateLoop = true;
        protected void StartMessageLoop()
        {
            if (_TerminateLoop == false)
                return;

            _TerminateLoop = false;

            new Thread(ThdRxLoop) { IsBackground = true }.Start();
            new Thread(ThdRxMsgProcessLoop) { IsBackground = true }.Start();
            new Thread(() => 
            {
                MAVLink.mavlink_heartbeat_t hb = new MAVLink.mavlink_heartbeat_t(0, (byte)MAVLink.MAV_TYPE.GCS, 0, 0, 0, 2);
                while (_TerminateLoop == false)
                {
                    if (_Connected && IsOpen)
                        SendMessage(MAVLink.MAVLINK_MSG_ID.HEARTBEAT, hb);
                    Thread.Sleep(HeartbeatInterval);
                }
            }) { IsBackground = true }.Start();
        }

        protected void StopMessageLoop()
        {
            _TerminateLoop = true;
        }

        protected void ThdRxLoop()
        {
            while (_TerminateLoop == false)
            {
                MAVLink.MAVLinkMessage msg = null;
                try
                {
                    msg = _Parse.ReadPacket(this);
                }
                catch (Exception ex)
                {
                    if (this is UdpStream == false && this.Connected == false)
                    {
                        RaiseLinkBrokenEvent();
                        StopMessageLoop();
                        break;
                    }
                }
                if (msg == null)
                {
                    continue;
                }
                else
                {
                    //Console.WriteLine($">>>>>>>>>>>>>>>> {(MAVLink.MAVLINK_MSG_ID)msg.msgid}");
                    //Console.Write("o");
                    _RxMessageQueue.Enqueue(msg);
                }
            }

            if (_TerminateLoop == false)
                Console.WriteLine("XXXXXXXXXXXXXXXXXXXXXX ThdRxLoop exited due to LINK BROKEN");
            else
                Console.WriteLine("...................... ThdRxLoop exited");
        }

        protected void ThdRxMsgProcessLoop()
        {
            MAVLink.MAVLinkMessage msg;
            int n = 0;
            while (_TerminateLoop == false)
            {
                // 维护一下订阅列表。lock 只能锁不同线程，锁不住相同线程。所以这里只能这样处理。
                n = _TempSubscribedMessageProcessMethods.Count;
                if (n > 0)
                {
                    var idList = _TempSubscribedMessageProcessMethods.Keys.ToArray();
                    foreach (var id in idList)
                    {
                        _SubscribedMessageProcessMethods[id] = _TempSubscribedMessageProcessMethods[id];
                        _TempSubscribedMessageProcessMethods.Remove(id);
                        Console.WriteLine("Process func added");
                    }
                }

                n = _TempRemoveList.Count;
                if(n>0)
                {
                    var idList = _TempRemoveList.ToArray();
                    foreach(var id in idList)
                    {
                        _TempRemoveList.Remove(id);
                        _SubscribedMessageProcessMethods.Remove(id);
                        Console.WriteLine("Process func removed");
                    }
                }

                //Console.Write("0");

                if (_RxMessageQueue.Count == 0)
                {
                    Thread.Sleep(10);
                    continue;
                }

                //Console.Write(".");

                while (_RxMessageQueue.Count > 0)
                {
                    if (_RxMessageQueue.TryDequeue(out msg) == false)
                        break;

                    if (_SubscribedMessageProcessMethods.Count == 0)
                        break;

                    //Console.Write("P");

                    foreach (var tuple in _SubscribedMessageProcessMethods.Values)
                    {
                        MAVLink.MAVLINK_MSG_ID msgId = tuple.Item1;
                        byte sysId = tuple.Item2;
                        byte compId = tuple.Item3;
                        var func = tuple.Item4;

                        if (sysId == 0 || (msg.sysid == sysId && msg.compid == compId))
                        {
                            if (msgId == (MAVLink.MAVLINK_MSG_ID)msg.msgid)
                                func(msg);
                        }
                    }
                }
                _LastDataTime = DateTime.Now;
            }

            if (_TerminateLoop == false)
                Console.WriteLine("XXXXXXXXXXXXXXXXXXXXXX ThdRxMsgProcessLoop exited due to _RxMessage ERROR");
            else
                Console.WriteLine("...................... ThdRxMsgProcessLoop exited");
        }

        int _SendFailure = 0;
        public void SendMessage(MAVLink.MAVLINK_MSG_ID msgType, object data)
        {
            byte[] bytes;
            if (MavlinkVersion == 2)
                bytes = _Parse.GenerateMAVLinkPacket20(msgType, data, false, GcsID);
            else
                bytes = _Parse.GenerateMAVLinkPacket10(msgType, data, GcsID);

            //MAVLink.MAVLinkMessage msg = new MAVLink.MAVLinkMessage(bytes);
            try
            {
                this.Write(bytes, 0, bytes.Length);
                _SendFailure = 0;
            }
            catch
            {
                _SendFailure++;
                if (this is UdpStream == false && _SendFailure > 5)
                {
                    RaiseLinkBrokenEvent();
                }
            }
            //Console.WriteLine($"Send msg:[{(MAVLink.MAVLINK_MSG_ID)msg.msgid}]: SEQ = {msg.seq}");
        }

        protected void RaiseLinkBrokenEvent()
        {
            _Connected = false;
            IsOpen = false;
            try
            {
                this.Close();
            }
            catch { }

            LinkBroken?.Invoke(this);
        }




        object _SubscribedMessageProcessMethodsLock = new object();
        Dictionary<int, Tuple<MAVLink.MAVLINK_MSG_ID, byte, byte, Func<MAVLink.MAVLinkMessage, bool>>> _SubscribedMessageProcessMethods = new Dictionary<int, Tuple<MAVLink.MAVLINK_MSG_ID, byte, byte, Func<MAVLink.MAVLinkMessage, bool>>>();
        Dictionary<int, Tuple<MAVLink.MAVLINK_MSG_ID, byte, byte, Func<MAVLink.MAVLinkMessage, bool>>> _TempSubscribedMessageProcessMethods = new Dictionary<int, Tuple<MAVLink.MAVLINK_MSG_ID, byte, byte, Func<MAVLink.MAVLinkMessage, bool>>>();
        List<int> _TempRemoveList = new List<int>();
        public int SubscribeMessage(MAVLink.MAVLINK_MSG_ID msgId, Func<MAVLink.MAVLinkMessage, bool> function, byte sysId, MAV_COMPONENT comp)
        {
            Tuple<MAVLink.MAVLINK_MSG_ID, byte, byte, Func<MAVLink.MAVLinkMessage, bool>> tuple =
                new Tuple<MAVLink.MAVLINK_MSG_ID, byte, byte, Func<MAVLink.MAVLinkMessage, bool>>(msgId, sysId, (byte)comp, function);
            int id = tuple.GetHashCode();

            if (_TempSubscribedMessageProcessMethods.ContainsKey(id) == false)
                _TempSubscribedMessageProcessMethods[id] = tuple;

            return id;
        }



        public void UnsubcribeMessage(int id)
        {
            _TempRemoveList.Add(id);
        }

        protected void RaiseLinkConnectedEvent(bool connected)
        {
            LinkConnected?.Invoke(this, connected);
        }
    }
}
